#include "uart.h"
#include <stdio.h>
using namespace std;

UART::UART():Serial()
{
    ros::NodeHandle nh;
    nh.param<std::string>("Port",_port,"/dev/ttyUSB0");
    nh.param<int>("Baudrate",_baudrate,57600);
    _to = Timeout::simpleTimeout(100);

}


void UART::localodomCallback(const nav_msgs::Odometry &msg)
{
    current_angle.x= msg.pose.pose.orientation.x;
    current_angle.y= msg.pose.pose.orientation.y;
    current_angle.z= msg.pose.pose.orientation.z;
    current_angle.w= msg.pose.pose.orientation.w;
    curr_angle = toEulerAngle(current_angle);
}

void UART::odometry_callback(const sensor_msgs::NavSatFix &current_info)
{
    Int2char lidar_pos_x_union, lidar_pos_y_union, lidar_pos_z_union, yaw_angle_union;
    lidar_pos_x_union.Int = current_info.latitude * 1e7;
    lidar_pos_y_union.Int = current_info.longitude * 1e7;
    lidar_pos_z_union.Int = current_info.altitude * 100;
    yaw_angle_union.Int = curr_angle.z*100;
    //header and payload
    //temp1[0~1] : header
    //temp1[2~3] : source  and   target
    //temp1[4] : message type
    //temp1[5] : message length
    //temp1[6~9] : not used
    
    uint8_t  temp1[38] = {0xb5,0x62,0xa1,0xb2,0xa1,0x26, 0x11,0x11,0x11,0x11};

    temp1[10] = lidar_pos_x_union.Char[0];
    temp1[11] = lidar_pos_x_union.Char[1];
    temp1[12] = lidar_pos_x_union.Char[2];
    temp1[13] = lidar_pos_x_union.Char[3];

    temp1[14] = lidar_pos_y_union.Char[0];
    temp1[15] = lidar_pos_y_union.Char[1];
    temp1[16] = lidar_pos_y_union.Char[2];
    temp1[17] = lidar_pos_y_union.Char[3];

    temp1[18] = lidar_pos_z_union.Char[0];
    temp1[19] = lidar_pos_z_union.Char[1];
    temp1[20] = lidar_pos_z_union.Char[2];
    temp1[21] = lidar_pos_z_union.Char[3];

    //yaw
    temp1[22] = yaw_angle_union.Char[0];
    temp1[23] = yaw_angle_union.Char[1];
    temp1[24] = yaw_angle_union.Char[2];
    temp1[25] = yaw_angle_union.Char[3];

    //vx
    temp1[26] = lidar_pos_y_union.Char[0];
    temp1[27] = lidar_pos_y_union.Char[1];
    temp1[28] = lidar_pos_y_union.Char[2];
    temp1[29] = lidar_pos_y_union.Char[3];

    //vy
    temp1[30] = lidar_pos_z_union.Char[0];
    temp1[31] = lidar_pos_z_union.Char[1];
    temp1[32] = lidar_pos_z_union.Char[2];
    temp1[33] = lidar_pos_z_union.Char[3];
 
    temp1[34] = 0x01; //command prepared
    //temp1[9] -> vehicle id
    temp1[9] = 0x01;
    //command list: 0x01->主机pub    0x02->从机pub   

    uint8_t crc = 0;

    for(uint8_t i=0;i<35;i++)
    {
        crc += temp1[i];
    }
    temp1[35] = crc;


    // crc check
    temp1[36] = 0x0d;
    temp1[37] = 0x0a;


    write(temp1, 38);
    std::cout<<"publish once dd.... "<<std::endl;
}


void UART::OpenSerial()
{
    try {
        this->open();
    } catch (serial::IOException& e) {
        ROS_ERROR_STREAM("Unable to open port.");
        return;
    }

    if(this->isOpen())
    {
        ROS_INFO_STREAM("/dev/ttyUSB0 is opened.");
    }
    else
    {
        return;
    }
}

uint8_t UART::CrcCheck(std::vector<uint8_t> buff,int len)
{
    uint8_t crc = 0;

    for(uint8_t i=0;i<len;i++)
    {
        crc += buff[i];
    }

    return crc;
}

geometry_msgs::Vector3 UART::toEulerAngle(geometry_msgs::Quaternion quat)
{
    geometry_msgs::Vector3 ans;

    tf::Matrix3x3 R_FLU2ENU(tf::Quaternion(quat.x, quat.y, quat.z, quat.w));
    R_FLU2ENU.getRPY(ans.x, ans.y, ans.z);
    return ans;
}


void UART::RunMain()
{   
    
    uint8_t package[256];
    uint8_t rx_head = 0;
    uint8_t rx_wr_index = 0;
    uint8_t b_rx_over = 0;
    serial_process_t _serial_process;
    OpenSerial();
    ros::Rate loop_rate(30);
    ros::NodeHandle nh2;
    ros::Subscriber _local_pos_sub2 = nh2.subscribe("/zhou/target_gps",10,&UART::odometry_callback,this);
    ros::Subscriber _local_odom_sub2 = nh2.subscribe("/mavros/local_position/odom",10,&UART::localodomCallback,this);
    ros::Publisher _global_pub2 = nh2.advertise<sensor_msgs::NavSatFix>("/vehicle/position/global",10);

    while(ros::ok())
    {

        //read serial
        size_t n = this->available();

        uint8_t buffer[256];
        n = this->read(buffer,n);//must! n<=256
        uint8_t b[256];
        for(uint8_t i=0;i<n;i++)
        {
            b[i] = buffer[i];
        }

        for(uint8_t i=0;i<n;i++)
        {
            if(b[i]==0xb5)
            {
                rx_head = 1;
                package[rx_wr_index++] = b[i];
            }
            else if(b[i]==0x62)
            {
                if(rx_head)
                {
                    rx_wr_index = 0;
                    b_rx_over = 0;
                }
                else
                {
                    package[rx_wr_index++] = b[i];
                    rx_head = 0;
                }

            }
            else
            {
                package[rx_wr_index++] = b[i];
                rx_head = 0;
                if(rx_wr_index>32) //>4*8 zanding
                {
                    if((rx_wr_index+2) == package[3] )
                    {

                        // std::cout<<"get the whole package."<<std::endl;
                        b_rx_over = 1;

                        _serial_process.rx_buffer.clear();
                        _serial_process.rx_buffer.push_back(0xb5);
                        _serial_process.rx_buffer.push_back(0x62);

                        for(uint8_t i=2; i<package[3];i++)
                        {
                            _serial_process.rx_buffer.push_back(package[i-2]);
                        }
                        _serial_process.index = package[3];
                        _serial_process.rx_over = 1;
                        memset(package,0,256);
                    }
                }
            }
            if(rx_wr_index>200)
                rx_wr_index=200;
        }

        if(_serial_process.rx_over==1)
        {
            std::vector<uint8_t> buffer;
            for(uint8_t i=0;i<_serial_process.index;i++)
            {

                buffer.push_back(_serial_process.rx_buffer[i]);
            }
            // std::cout<<(int)buffer[0]<<std::endl;
            if(buffer[0]==0xb5 && buffer[1]==0x62 && buffer[_serial_process.index-2]==0x0d && buffer[_serial_process.index-1]==0x0a)
            {
                // std::cout<<"check header successfully!"<<std::endl;
                //crc check
                uint8_t crc = CrcCheck(buffer,_serial_process.index-3);
                if(crc == buffer[_serial_process.index-3])
                {
                    uint8_t temp[_serial_process.index];
                    for(uint8_t i=0;i<_serial_process.index;i++)
                    {
                        temp[i] = buffer[i];
                    }
                    std::cout<<"check crc successfully!"<<std::endl;

                    memcpy(&_gpsRec,temp+10,25);
                    //publish gps
                    sensor_msgs::NavSatFix gps_pos;
                    gps_pos.position_covariance[0] = temp[9];//_gpsRec.vehicleid;
                    gps_pos.latitude = _gpsRec.latitude/10000000.0;
                    gps_pos.longitude = _gpsRec.longitude/10000000.0;
                    gps_pos.altitude = _gpsRec.altitude/100.0;
                    gps_pos.position_covariance[1] = _gpsRec.target_lat/10000000.0;
                    gps_pos.position_covariance[2] = _gpsRec.target_lon/10000000.0;
                    gps_pos.position_covariance[3] = _gpsRec.target_alt/100.0;
                    //position_covariance[0]->master or follower , position_covariance[1]->vehicle id
                    gps_pos.position_covariance[4] = _gpsRec.command;
                    if(gps_pos.position_covariance[0]==2)
                    {
                        _global_pub2.publish(gps_pos);
                        std::cout<<"receive message from  vehicle !   "<<std::endl;
                    }
                    

                }
            }

            _serial_process.rx_over = 0;
        }


        ros::spinOnce();
        loop_rate.sleep();
    }
}
